#include  "ros/ros.h"
#include  "tf2_ros/transform_listener.h"
#include  "tf2_ros/buffer.h"
#include "geometry_msgs/PointStamped.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
int main(int argc, char **argv)
{
    setlocale(LC_ALL, "");
    ros::init(argc, argv, "static_sub");
    ros::NodeHandle n;
    // 3 创建订阅对象； 订阅坐标系相对关系
    // 3-1 创建buffer缓存
    tf2_ros::Buffer buffer;
    // 3-2 创建监听对象（监听对象可以将订阅的数据存入buffer）
    tf2_ros::TransformListener listener(buffer);
    // 组织座标点数据
    geometry_msgs::PointStamped point;
    point.header.frame_id = "laser";
    point.header.stamp = ros::Time::now();
    point.point.x = 2.0;
    point.point.y = 3.0;    
    point.point.z = 5.0;

    // 5 转换算法，需要调用tf内置实现；
    ros::Rate rate(10);
    // ros::Duration(2.0).sleep();
    while (ros::ok())
    {
        // 核心代码实现 将point转换成相对于base_link的座标点
        geometry_msgs::PointStamped ps_out;
        /*
        调用buffer的转换函数
        参数1：被转换的座标点
        参数2：目标坐标系
        返回值：输出的座标点

        PS1：调用时必须包含头文件"tf2_geometry_msgs/tf2_geometry_msgs.h"
        PS2：运行时存在问题，base_link不存在，原因是转换时，订阅数据是个耗时操作
            调用的数据还未存到buffer中，可以sleep一下以后再转换
            方案2：进行异常处理，使用try
        */
        try
        {
            ps_out = buffer.transform(point, "base_link");
            ROS_INFO("point: (%.2f, %.2f, %.2f) %s", ps_out.point.x, 
                                              ps_out.point.y,
                                              ps_out.point.z,
                                              ps_out.header.frame_id.c_str());
        }
        catch (const std::exception& e)
        {
            std::cerr << e.what() << '\n';
        }        
        rate.sleep();
        ros::spinOnce();

    }

    return 0;
}